// Copyright 2020 Kitware SAS
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "TestHelpers.h"
#include "vtkLidarReader.h"
#include "vtkVelodyneMetaPacketInterpreter.h"

#include <vtkNew.h>
#include <vtkPointData.h>

/**
 * @brief This test does not require prediction values, thus the pcap should be easily changed.
 * @param pcapFileName The Velodyne pcap file containing points covering all 360° in plane
 * @param calibrationFileName The XML Velodyne sensor calibration file
 * @return 0 on success, 1 on failure
 */
int main(int argc, char* argv[])
{
  if (argc < 3)
  {
    std::cerr << "Wrong number of arguments. Usage: TestTimeFraming <pcapFileName> <calibrationFileName>" << std::endl;

    return 1;
  }

  const double frameDuration1 = 0.1;
  const double frameDuration2 = 0.2;

  std::string pcapFileName = argv[1];
  std::string calibrationFileName = argv[2];

  std::cout << "-------------------------------------------------------------------------" << std::endl
            << "Pcap :\t" << pcapFileName << std::endl
            << "Calibration:\t" << calibrationFileName << std::endl
            << "-------------------------------------------------------------------------" << std::endl;

  // Generate a Lidar reader
  vtkNew<vtkLidarReader> reader;
  auto interp = vtkSmartPointer<vtkVelodyneMetaPacketInterpreter>::New();
  interp->SetIgnoreEmptyFrames(true);
  interp->SetIgnoreZeroDistances(true);
  interp->SetEnableAdvancedArrays(true);
  reader->SetInterpreter(interp);
  reader->SetShowFirstAndLastFrame(true);
  reader->SetFileName(pcapFileName);
  reader->SetCalibrationFileName(calibrationFileName);
  reader->Update();

  // Check if we can get frames using the interpreter framing
  if (reader->GetNumberOfFrames() == 0)
  {
      std::cerr << "error: the reader ouput 0 frame, please check paths and pcap file" << std::endl;
      return 1;
  }

  // Check that we can change the framing method
  reader->GetInterpreter()->SetFramingMethod(NETWORK_PACKET_TIME_FRAMING);

  // Set First frame duration
  reader->GetInterpreter()->SetFrameDuration_s(frameDuration1);
  reader->Update();
  int frameCount1 = reader->GetNumberOfFrames();
  if (frameCount1 == 0)
  {
      std::cerr << "error: frames are expected when using NETWORK_PACKET_TIME_FRAMING" << std::endl;
      return 1;
  }

  // Check second frame duration
  reader->GetInterpreter()->SetFrameDuration_s(frameDuration2);
  reader->Update();
  int frameCount2 = reader->GetNumberOfFrames();
  if (frameCount2 == 0)
  {
      std::cerr << "error: frames are expected when using NETWORK_PACKET_TIME_FRAMING" << std::endl;
      return 1;
  }

  double length1 = frameDuration1 * frameCount1;
  double length2 = frameDuration2 * frameCount2;
  if (std::abs(length1 - length2) > 2.0 * std::max(frameDuration1, frameDuration2))
  {
      std::cerr << "error: frame count and frame duration do not change accordingly" << std::endl;
      return 1;
  }

  reader->Open();
  // Check that the frame has expected duration when using lidar point time
  // ! This assumes the frame contains points covering all 360° in plane
  vtkSmartPointer<vtkPolyData> frame = reader->GetFrame(1);
  reader->Close();

  double start = 0.0;
  double end = 0.0;

  if (!GetFrameTimeRange(frame, start, end))
  {
    std::cerr << "error: could not get frame time range" << std::endl;
    return 1;
  }

  double effectiveDuration = end - start;
  double relError = std::abs(effectiveDuration - frameDuration2) / frameDuration2;
  if (relError > 0.1)
  {
    std::cerr << "error: time range of points in frame differs too much from expected time range" << std::endl;
    return 1;
  }

  return  0;
}

